home *** CD-ROM | disk | FTP | other *** search
/ Computer Shopper 242 / Issue 242 - April 2008 - DPCS0408DVD.ISO / Software Money Savers / VirtualDub / Source / VirtualDub-1.7.7-src.7z / src / VDLib / source / fft.cpp next >
Encoding:
C/C++ Source or Header  |  2006-11-05  |  13.7 KB  |  606 lines
  1. //    VirtualDub - Video processing and capture application
  2. //    Application helper library
  3. //    Copyright (C) 1998-2006 Avery Lee
  4. //
  5. //    This program is free software; you can redistribute it and/or modify
  6. //    it under the terms of the GNU General Public License as published by
  7. //    the Free Software Foundation; either version 2 of the License, or
  8. //    (at your option) any later version.
  9. //
  10. //    This program is distributed in the hope that it will be useful,
  11. //    but WITHOUT ANY WARRANTY; without even the implied warranty of
  12. //    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13. //    GNU General Public License for more details.
  14. //
  15. //    You should have received a copy of the GNU General Public License
  16. //    along with this program; if not, write to the Free Software
  17. //    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  
  19. #include "stdafx.h"
  20. #include <vd2/system/math.h>
  21. #include <vd2/system/cpuaccel.h>
  22. #include <vd2/system/error.h>
  23. #include <vd2/VDLib/fft.h>
  24.  
  25. #include "fft_radix2.inl"
  26. #include "fft_radix4.inl"
  27. #ifdef _M_IX86
  28.     #include "fft_sse.inl"
  29. #endif
  30.  
  31. inline uint32 VDRevBits32(uint32 x) {
  32.     uint32 y;
  33.  
  34.     y = (x>>16) + (x<<16);
  35.     x = ((y & 0xff00ff00) >> 8) + ((y & 0x00ff00ff) << 8);
  36.     y = ((x & 0xf0f0f0f0) >> 4) + ((x & 0x0f0f0f0f) << 4);
  37.     x = ((y & 0xcccccccc) >> 2) + ((y & 0x33333333) << 2);
  38.     y = ((x & 0xaaaaaaaa) >> 1) + ((x & 0x55555555) << 1);
  39.  
  40.     return y;
  41. }
  42.  
  43. inline uint32 VDRevBits(uint32 v, unsigned bits) {
  44.     return VDRevBits32(v) >> (32 - bits);
  45. }
  46.  
  47. void VDMakePermuteTable(uint32 *dst0, unsigned bits) {
  48.     unsigned N = 1<<bits;
  49.     uint32 *dst = dst0;
  50.  
  51.     for(unsigned i=0; i<N; ++i) {
  52.         unsigned j = VDRevBits(i, bits);
  53.  
  54.         if (i < j) {
  55.             dst[0] = i;
  56.             dst[1] = j;
  57.             dst += 2;
  58.         }
  59.     }
  60.  
  61.     VDASSERT((uint32)(dst - dst0) == N - (1 << ((bits + 1) >> 1)));
  62. }
  63.  
  64. void VDPermuteRevBitsComplex(float *v, unsigned bits, const uint32 *permuteTable) {
  65.     unsigned N = 1<<bits;
  66.     unsigned count = N - (1 << ((bits + 1) >> 1));
  67.  
  68.     for(unsigned i=0; i<count; i += 2) {
  69.         const unsigned x = permuteTable[0];
  70.         const unsigned y = permuteTable[1];
  71.         permuteTable += 2;
  72.  
  73.         const unsigned i2 = x+x;
  74.         const unsigned j2 = y+y;
  75.         float tmp;
  76.  
  77.         tmp = v[i2  ]; v[i2  ] = v[j2  ]; v[j2  ] = tmp;
  78.         tmp = v[i2+1]; v[i2+1] = v[j2+1]; v[j2+1] = tmp;
  79.     }
  80. }
  81.  
  82. void VDCreateRaisedCosineWindow(float *dst, int n) {
  83.     const double twopi_over_n = nsVDMath::krTwoPi / n;
  84.     const double scalefac = 0.5 / n;
  85.  
  86.     for(int i=0; i<n; ++i)
  87.         dst[i] = (float)(scalefac * (1.0 - cos(twopi_over_n * (i+0.5))));
  88. }
  89.  
  90. /// Decimation-in-time FFT. Permute before calling.
  91. void VDComputeComplexFFT_DIT(float *p, unsigned bits) {
  92.     if (bits >= 2) {
  93.         unsigned k = 3;
  94.  
  95.         VDComputeComplexFFT_DIT_radix4_iter0(p, bits);
  96.  
  97.         for(; k<bits; k += 2)
  98.             VDComputeComplexFFT_DIT_radix4(p, bits, k);
  99.     }
  100.  
  101.     if (bits & 1)
  102.         VDComputeComplexFFT_DIT_radix2(p, bits, bits);
  103. }
  104.  
  105. void VDComputeComplexFFT_DIT_table(float *p, unsigned bits, const float *table) {
  106.     if (bits >= 2) {
  107. #ifdef _M_IX86
  108.         if (SSE_enabled) {
  109.             VDComputeComplexFFT_DIT_radix4_iter0_SSE(p, bits);
  110.             for(unsigned k = 3; k<bits; k += 2)
  111.             {
  112.                 if (k < bits-1)
  113.                     VDComputeComplexFFT_DIT_radix4_table2x_SSE(p, bits, k, table, 6 << (bits - k - 1));
  114.                 else
  115.                     VDComputeComplexFFT_DIT_radix4_table_SSE(p, bits, k, table, 6 << (bits - k - 1));
  116.             }
  117.         } else
  118. #endif
  119.         {
  120.             VDComputeComplexFFT_DIT_radix4_iter0(p, bits);
  121.             for(unsigned k = 3; k<bits; k += 2)
  122.                 VDComputeComplexFFT_DIT_radix4_table(p, bits, k, table, 6 << (bits - k - 1));
  123.         }
  124.     }
  125.  
  126.     if (bits & 1)
  127.         VDComputeComplexFFT_DIT_radix2(p, bits, bits);
  128. }
  129.  
  130. void VDComputeRealFFT(float *p, unsigned bits) {
  131.     // transform as if it were complex
  132.     VDComputeComplexFFT_DIT(p, bits-1);
  133.  
  134.     // do even-odd decomposition and last radix-2 step
  135.     const unsigned N = 1 << bits;        // number of elements in FT
  136.     float *p0 = p + 2;
  137.     float *p1 = p + N - 2;
  138.     float angstep = nsVDMath::kfPi / (float)(1 << bits) * 2.0f;
  139.     float scrot = cosf(angstep);
  140.     float ssrot = sinf(angstep);
  141.     float sc = scrot;
  142.     float ss = ssrot;
  143.  
  144.     for(unsigned i=4; i<N; i += 4) {
  145.         float r0 = p0[0];
  146.         float i0 = p0[1];
  147.         float r1 = p1[0];
  148.         float i1 = p1[1];
  149.  
  150.         float r2 = (r0 + r1) * 0.5f;
  151.         float i2 = (i0 - i1) * 0.5f;
  152.         float r3 = (i0 + i1) * 0.5f;
  153.         float i3 = (r0 - r1) * 0.5f;
  154.  
  155.         float r4 = r3*sc - i3*ss;    // rotate by +ang (== rot by 180-ang)
  156.         float i4 = r3*ss + i3*sc;
  157.  
  158.         p0[0] = r2 + r4;
  159.         p0[1] = i2 - i4;
  160.         p1[0] = r2 - r4;
  161.         p1[1] = -i2 - i4;
  162.  
  163.         float sc2 = sc*scrot - ss*ssrot;
  164.         ss = sc*ssrot + ss*scrot;
  165.         sc = sc2;
  166.  
  167.         p0 += 2;
  168.         p1 -= 2;
  169.     }
  170.  
  171.     p[(N>>1)+1] = -p[(N>>1)+1];
  172.  
  173.     // butterfly on 0 and N/2
  174.     float rmin = p[0];
  175.     float rmax = p[1];
  176.  
  177.     p[0] = rmin + rmax;
  178.     p[1] = rmin - rmax;
  179. }
  180.  
  181. void VDComputeComplexFFT_DIF(float *p, unsigned bits) {
  182.     if (bits & 1)
  183.         VDComputeComplexFFT_DIF_radix2(p, bits, bits);
  184.  
  185.     if (bits >= 2) {
  186.         for(int k=(bits&~1)-1; k>=3; k -= 2)
  187.             VDComputeComplexFFT_DIF_radix4(p, bits, (unsigned)k);
  188.  
  189.         VDComputeComplexFFT_DIF_radix4_iter0(p, bits);
  190.     }
  191. }
  192.  
  193. void VDComputeRealIFFT(float *p, unsigned bits) {
  194.     const unsigned N = 1 << bits;        // number of elements in FT
  195.     p[(N>>1)+1] = -p[(N>>1)+1];
  196.  
  197.     // do even-odd decomposition and last radix-2 step
  198.     float *p0 = p + 2;
  199.     float *p1 = p + N - 2;
  200.     float angstep = -nsVDMath::kfPi / (float)(1 << bits) * 2.0f;
  201.     float scrot = cosf(angstep);
  202.     float ssrot = sinf(angstep);
  203.     float sc = scrot;
  204.     float ss = ssrot;
  205.     for(unsigned i=4; i<N; i += 4) {
  206.         float r0 = p0[0];            // F(k)
  207.         float i0 = p0[1];
  208.         float r1 = p1[0];            // F(N/2-k)
  209.         float i1 = p1[1];
  210.  
  211.         float r2 = (r0 + r1)*0.5f;    // Fe(k) = .5 * (F(k) + F(N/2-k)*)
  212.         float i2 = (i0 - i1)*0.5f;
  213.         float r4 = (r0 - r1)*0.5f;
  214.         float i4 = (-i1 - i0)*0.5f;
  215.  
  216.         float r3 = r4*sc - i4*ss;    // Fo(k) = .5 * (F(k) - F(N/2-k)*)*rot
  217.         float i3 = r4*ss + i4*sc;
  218.  
  219.         // Z(k) = Fe(k) + jFo(k)
  220.         p0[0] = r2 + i3;
  221.         p0[1] = r3 + i2;
  222.         p1[0] = r2 - i3;
  223.         p1[1] = r3 - i2;
  224.  
  225.         float sc2 = sc*scrot - ss*ssrot;
  226.         ss = sc*ssrot + ss*scrot;
  227.         sc = sc2;
  228.  
  229.         p0 += 2;
  230.         p1 -= 2;
  231.     }
  232.  
  233.     // butterfly on 0 and N/2
  234.     float rmin = p[0];
  235.     float rmax = p[1];
  236.  
  237.     p[0] = (rmin + rmax);
  238.     p[1] = (rmin - rmax);
  239.  
  240.     // transform as if it were complex
  241.     VDComputeComplexFFT_DIF(p, bits-1);
  242. }
  243.  
  244. void VDComputeComplexFFT_Reference(float *out, float *in, unsigned bits, double sign) {
  245.     unsigned N = 1 << bits;
  246.     double angstep = sign*2.0*nsVDMath::kfPi / (double)N;
  247.  
  248.     for(unsigned i=0; i<N; ++i) {
  249.         double ang = 0;
  250.         double angstep2 = angstep * i; 
  251.         double sumr = 0;
  252.         double sumi = 0;
  253.  
  254.         for(unsigned j=0; j<N; ++j) {
  255.             double c = cos(ang);
  256.             double s = sin(ang);
  257.             double r = in[j*2+0];
  258.             double i = in[j*2+1];
  259.  
  260.             sumr += c*r - s*i;
  261.             sumi += c*i + s*r;
  262.  
  263.             ang += angstep2;
  264.         }
  265.  
  266.         out[0] = (float)sumr;
  267.         out[1] = (float)sumi;
  268.         out += 2;
  269.     }
  270. }
  271.  
  272. /////////////////////////////////////////////////////////////////////////////////////////////////////
  273.  
  274. namespace {
  275.     void sumdiff(float& x, float& y) {
  276.         float t = x-y;
  277.         x = x+y;
  278.         y = t;
  279.     }
  280.  
  281.     void rotate(float& x, float& y) {
  282.         float a = x;
  283.         float b = y;
  284.  
  285.         x = b;
  286.         y = -a;
  287.     }
  288.  
  289.     void rotatei(float& x, float& y) {
  290.         float a = x;
  291.         float b = y;
  292.  
  293.         x = -b;
  294.         y = a;
  295.     }
  296.  
  297.     void cmult(float& x, float& y, float r, float i) {
  298.         float a = x;
  299.         float b = y;
  300.  
  301.         x = a*r - b*i;
  302.         y = a*i + b*r;
  303.     }
  304. }
  305.  
  306. VDRealFFT::VDRealFFT() 
  307.     : mpPermuteTable(NULL)
  308.     , mpWeightTable(NULL)
  309. {
  310. }
  311.  
  312. VDRealFFT::VDRealFFT(unsigned bits) {
  313.     Init(bits);
  314. }
  315.  
  316. VDRealFFT::~VDRealFFT() {
  317.     Shutdown();
  318. }
  319.  
  320. void VDRealFFT::Init(unsigned bits) {
  321.     Shutdown();
  322.  
  323.     mBits = bits;
  324.  
  325.     uint32 N = 1 << bits;
  326.     uint32 N2 = N >> 1;
  327.     uint32 N4 = N >> 2;
  328.  
  329.     mpPermuteTable = new uint32[N2];
  330.     mpWeightTable = new float[N*4];
  331.  
  332.     VDMakePermuteTable(mpPermuteTable, bits - 1);
  333.  
  334.     float *p = mpWeightTable;
  335.  
  336.     // compute weight table for real-to-complex conversion (N/4 points, 1/N spacing)
  337.     float step = 6.283185307179586476925286766559f / (1 << bits);
  338.     for(unsigned i=1; i<N4; ++i) {
  339.         *p++ = cosf(step * i);
  340.         *p++ = sinf(step * i);
  341.     }
  342.  
  343.     // compute weights for radix-2/radix-4 FFT (initial: N/2 points, 1/2N spacing){
  344.     unsigned size = 1 << (bits - 1);
  345.     step *= 2.0f;
  346.  
  347.     for(unsigned i=0; i<size; ++i) {
  348.         *p++ = cosf(step * i);
  349.         *p++ = sinf(step * i);
  350.         *p++ = cosf(step * i * 2);
  351.         *p++ = sinf(step * i * 2);
  352.         *p++ = cosf(step * i * 3);
  353.         *p++ = sinf(step * i * 3);
  354.     }
  355. }
  356.  
  357. void VDRealFFT::Shutdown() {
  358.     if (mpPermuteTable) {
  359.         delete[] mpPermuteTable;
  360.         mpPermuteTable = NULL;
  361.     }
  362.  
  363.     if (mpWeightTable) {
  364.         delete[] mpWeightTable;
  365.         mpWeightTable = NULL;
  366.     }
  367. }
  368.  
  369. void VDRealFFT::ComputeRealFFT(float *p) {
  370.     const unsigned N = 1 << mBits;        // number of elements in FT
  371.     const float *w = mpWeightTable;
  372.  
  373.     // permute
  374.     VDPermuteRevBitsComplex(p, mBits-1, mpPermuteTable);
  375.  
  376.     // transform as if it were complex
  377.     VDComputeComplexFFT_DIT_table(p, mBits-1, w + (N>>1) - 2);
  378.  
  379.     // do even-odd decomposition and last radix-2 step
  380.     float *p0 = p + 2;
  381.     float *p1 = p + N - 2;
  382.  
  383.     for(unsigned i=4; i<N; i += 4) {
  384.         float r0 = p0[0];
  385.         float i0 = p0[1];
  386.         float r1 = p1[0];
  387.         float i1 = p1[1];
  388.  
  389.         float r2 = (r0 + r1) * 0.5f;
  390.         float i2 = (i0 - i1) * 0.5f;
  391.         float r3 = (i0 + i1) * 0.5f;
  392.         float i3 = (r0 - r1) * 0.5f;
  393.  
  394.         float sc = *w++;
  395.         float ss = *w++;
  396.         float r4 = r3*sc - i3*ss;    // rotate by +ang (== rot by 180-ang)
  397.         float i4 = r3*ss + i3*sc;
  398.  
  399.         p0[0] = r2 + r4;
  400.         p0[1] = i2 - i4;
  401.         p1[0] = r2 - r4;
  402.         p1[1] = -i2 - i4;
  403.  
  404.         p0 += 2;
  405.         p1 -= 2;
  406.     }
  407.  
  408.     p[(N>>1)+1] = -p[(N>>1)+1];
  409.  
  410.     // butterfly on 0 and N/2
  411.     float rmin = p[0];
  412.     float rmax = p[1];
  413.  
  414.     p[0] = rmin + rmax;
  415.     p[1] = rmin - rmax;
  416. }
  417.  
  418. void VDRealFFT::ComputeRealIFFT(float *p) {
  419.     const unsigned N = 1 << mBits;        // number of elements in FT
  420.     const float *w = mpWeightTable;
  421.     p[(N>>1)+1] = -p[(N>>1)+1];
  422.  
  423.     // do even-odd decomposition and last radix-2 step
  424.     float *p0 = p + 2;
  425.     float *p1 = p + N - 2;
  426.     for(unsigned i=4; i<N; i += 4) {
  427.         float r0 = p0[0];            // F(k)
  428.         float i0 = p0[1];
  429.         float r1 = p1[0];            // F(N/2-k)
  430.         float i1 = p1[1];
  431.  
  432.         float r2 = (r0 + r1)*0.5f;    // Fe(k) = .5 * (F(k) + F(N/2-k)*)
  433.         float i2 = (i0 - i1)*0.5f;
  434.         float r4 = (r0 - r1)*0.5f;
  435.         float i4 = (-i1 - i0)*0.5f;
  436.  
  437.         float sc = *w++;
  438.         float ss = -*w++;
  439.         float r3 = r4*sc - i4*ss;    // Fo(k) = .5 * (F(k) - F(N/2-k)*)*rot
  440.         float i3 = r4*ss + i4*sc;
  441.  
  442.         // Z(k) = Fe(k) + jFo(k)
  443.         p0[0] = r2 + i3;
  444.         p0[1] = r3 + i2;
  445.         p1[0] = r2 - i3;
  446.         p1[1] = r3 - i2;
  447.  
  448.         p0 += 2;
  449.         p1 -= 2;
  450.     }
  451.  
  452.     // butterfly on 0 and N/2
  453.     float rmin = p[0];
  454.     float rmax = p[1];
  455.  
  456.     p[0] = (rmin + rmax)*0.5f;
  457.     p[1] = (rmin - rmax)*0.5f;
  458.  
  459.     // transform as if it were complex
  460.     VDComputeComplexFFT_DIF(p, mBits-1);
  461.  
  462.     // permute
  463.     VDPermuteRevBitsComplex(p, mBits-1, mpPermuteTable);
  464. }
  465.  
  466. ///////////////////////////////////////////////////////////////////////////
  467.  
  468. VDRollingRealFFT::VDRollingRealFFT()
  469.     : mPoints(0)
  470.     , mBufferLevel(0)
  471.     , mpWindow(NULL)
  472.     , mpSampleBuffer(NULL)
  473.     , mpTempArea(NULL)
  474. {
  475. }
  476.  
  477. VDRollingRealFFT::VDRollingRealFFT(unsigned bits)
  478.     : mPoints(0)
  479.     , mBufferLevel(0)
  480.     , mpWindow(NULL)
  481.     , mpSampleBuffer(NULL)
  482.     , mpTempArea(NULL)
  483. {
  484.     Init(bits);
  485. }
  486.  
  487. VDRollingRealFFT::~VDRollingRealFFT() {
  488.     Shutdown();
  489. }
  490.  
  491. void VDRollingRealFFT::Init(unsigned bits) {
  492.     mPoints = 1 << bits;
  493.  
  494.     mpWindow = (float *)VDAlignedMalloc(mPoints*sizeof(float), 16);
  495.     if (!mpWindow)
  496.         throw MyMemoryError();
  497.  
  498.     const float step = 2*nsVDMath::kfPi / (sint32)mPoints;
  499.     const float scalefac = 0.5f / mPoints;
  500.  
  501.     for(uint32 i=0; i<mPoints; ++i)
  502.         mpWindow[i] = scalefac * (1.0f - cosf(step * ((sint32)i+0.5f)));
  503.  
  504.     mpSampleBuffer = (float *)VDAlignedMalloc(mPoints*sizeof(float), 16);
  505.     if (!mpSampleBuffer)
  506.         throw MyMemoryError();
  507.  
  508.     mpTempArea = (float *)VDAlignedMalloc(mPoints*sizeof(float), 16);
  509.     if (!mpTempArea)
  510.         throw MyMemoryError();
  511.  
  512.     mFFT.Init(bits);
  513.  
  514.     Clear();
  515. }
  516.  
  517. void VDRollingRealFFT::Shutdown() {
  518.     if (mpWindow) {
  519.         VDAlignedFree(mpWindow);
  520.         mpWindow = NULL;
  521.     }
  522.  
  523.     if (mpTempArea) {
  524.         VDAlignedFree(mpTempArea);
  525.         mpTempArea = NULL;
  526.     }
  527.  
  528.     if (mpSampleBuffer) {
  529.         VDAlignedFree(mpSampleBuffer);
  530.         mpSampleBuffer = NULL;
  531.     }
  532.  
  533.     mFFT.Shutdown();
  534. }
  535.  
  536. void VDRollingRealFFT::Clear() {
  537.     memset(mpSampleBuffer, 0, sizeof(*mpSampleBuffer) * mPoints);
  538.     mBufferLevel = mPoints;
  539. }
  540.  
  541. void VDRollingRealFFT::Advance(uint32 samples) {
  542.     if (samples >= mBufferLevel)
  543.         mBufferLevel = 0;
  544.     else {
  545.         mBufferLevel -= samples;
  546.         memmove(mpSampleBuffer, mpSampleBuffer + samples, mBufferLevel * sizeof(*mpSampleBuffer));
  547.     }
  548. }
  549.  
  550. void VDRollingRealFFT::CopyIn8U(const uint8 *src, uint32 count, ptrdiff_t stride) {
  551.     if (count <= 0)
  552.         return;
  553.  
  554.     if (count > mPoints) {
  555.         src += (count-mPoints)*stride;
  556.         count = mPoints;
  557.     }
  558.  
  559.     if (count > mPoints - mBufferLevel)
  560.         Advance(count - (mPoints - mBufferLevel));
  561.  
  562.     float *dst = mpSampleBuffer + mBufferLevel;
  563.     mBufferLevel += count;
  564.     do {
  565.         *dst++ = ((sint32)*src - 0x80) * (1.0f / 128.0f);
  566.         src += stride;
  567.     } while(--count);
  568. }
  569.  
  570. void VDRollingRealFFT::CopyIn16S(const sint16 *src, uint32 count, ptrdiff_t stride) {
  571.     if (count <= 0)
  572.         return;
  573.  
  574.     if (count > mPoints) {
  575.         src = (const sint16 *)((const char *)src + (count-mPoints)*stride);
  576.         count = mPoints;
  577.     }
  578.  
  579.     if (count > mPoints - mBufferLevel)
  580.         Advance(count - (mPoints - mBufferLevel));
  581.  
  582.     float *dst = mpSampleBuffer + mBufferLevel;
  583.     mBufferLevel += count;
  584.     do {
  585.         *dst++ = *src * (1.0f / 32768.0f);
  586.         src = (const signed short *)((const char *)src + stride);
  587.     } while(--count);
  588. }
  589.  
  590. void VDRollingRealFFT::Transform() {
  591.     for(uint32 i=0; i<mPoints; ++i)
  592.         mpTempArea[i] = mpSampleBuffer[i] * mpWindow[i];
  593.  
  594.     mFFT.ComputeRealFFT(mpTempArea);
  595. }
  596.  
  597. float VDRollingRealFFT::GetPower(int bin) const {
  598.     VDASSERT((unsigned)bin < mPoints);
  599.     bin += bin;
  600.  
  601.     float re = mpTempArea[bin];
  602.     float im = mpTempArea[bin+1];
  603.     return re*re + im*im;
  604. }
  605.  
  606.